//
// File: rpy_filter.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 15-Dec-2024 12:01:49
//

// Include Files
#include "rpy_filter.h"
#include <algorithm>
#include <cmath>
#include <cstring>

// Variable Definitions
static double roll_pre;

static double pitch_pre;

static double yaw_pre;

static double P[9];

// Function Definitions
//
// Arguments    : double roll
//                double pitch
//                double yaw
//                double R
//                double Q
//                double *roll_f
//                double *pitch_f
//                double *yaw_f
// Return Type  : void
//
void rpy_filter(double roll, double pitch, double yaw, double R, double Q,
                double *roll_f, double *pitch_f, double *yaw_f)
{
  static const signed char b_a[9]{1, 0, 0, 0, 1, 0, 0, 0, 1};
  double B[9];
  double Qdi[9];
  double Rdi[9];
  double a[9];
  double b_roll[3];
  double rpy_hat[3];
  double a21;
  double d;
  double maxval;
  int Rdi_tmp;
  int r1;
  int r2;
  int r3;
  int rpy_hat_tmp;
  int rtemp;
  std::memset(&Rdi[0], 0, 9U * sizeof(double));
  Rdi[0] = R;
  Rdi[4] = R;
  Rdi[8] = R;
  rpy_hat[0] = Q;
  rpy_hat[1] = Q;
  rpy_hat[2] = Q;
  std::memset(&Qdi[0], 0, 9U * sizeof(double));
  //  初始化状态转移矩阵A和观测矩阵C
  //  预测步骤
  b_roll[0] = roll_pre;
  b_roll[1] = pitch_pre;
  b_roll[2] = yaw_pre;
  for (rtemp = 0; rtemp < 3; rtemp++) {
    Qdi[rtemp + 3 * rtemp] = rpy_hat[rtemp];
    maxval = 0.0;
    for (Rdi_tmp = 0; Rdi_tmp < 3; Rdi_tmp++) {
      rpy_hat_tmp = rtemp + 3 * Rdi_tmp;
      maxval += static_cast<double>(b_a[rpy_hat_tmp]) * b_roll[Rdi_tmp];
      a[rpy_hat_tmp] =
          (static_cast<double>(b_a[rtemp]) * P[3 * Rdi_tmp] +
           static_cast<double>(b_a[rtemp + 3]) * P[3 * Rdi_tmp + 1]) +
          static_cast<double>(b_a[rtemp + 6]) * P[3 * Rdi_tmp + 2];
    }
    rpy_hat[rtemp] = maxval;
  }
  //  更新步骤
  for (Rdi_tmp = 0; Rdi_tmp < 3; Rdi_tmp++) {
    maxval = a[Rdi_tmp];
    a21 = a[Rdi_tmp + 3];
    d = a[Rdi_tmp + 6];
    for (rpy_hat_tmp = 0; rpy_hat_tmp < 3; rpy_hat_tmp++) {
      rtemp = Rdi_tmp + 3 * rpy_hat_tmp;
      P[rtemp] = ((maxval * static_cast<double>(b_a[3 * rpy_hat_tmp]) +
                   a21 * static_cast<double>(b_a[3 * rpy_hat_tmp + 1])) +
                  d * static_cast<double>(b_a[3 * rpy_hat_tmp + 2])) +
                 Qdi[rtemp];
    }
    maxval = P[Rdi_tmp];
    a21 = P[Rdi_tmp + 3];
    d = P[Rdi_tmp + 6];
    for (rpy_hat_tmp = 0; rpy_hat_tmp < 3; rpy_hat_tmp++) {
      Qdi[Rdi_tmp + 3 * rpy_hat_tmp] =
          (maxval * static_cast<double>(b_a[3 * rpy_hat_tmp]) +
           a21 * static_cast<double>(b_a[3 * rpy_hat_tmp + 1])) +
          d * static_cast<double>(b_a[3 * rpy_hat_tmp + 2]);
    }
  }
  for (Rdi_tmp = 0; Rdi_tmp < 3; Rdi_tmp++) {
    signed char i;
    signed char i1;
    signed char i2;
    i = b_a[Rdi_tmp];
    i1 = b_a[Rdi_tmp + 3];
    i2 = b_a[Rdi_tmp + 6];
    for (rpy_hat_tmp = 0; rpy_hat_tmp < 3; rpy_hat_tmp++) {
      a[Rdi_tmp + 3 * rpy_hat_tmp] =
          (static_cast<double>(i) * P[3 * rpy_hat_tmp] +
           static_cast<double>(i1) * P[3 * rpy_hat_tmp + 1]) +
          static_cast<double>(i2) * P[3 * rpy_hat_tmp + 2];
    }
    maxval = a[Rdi_tmp];
    a21 = a[Rdi_tmp + 3];
    d = a[Rdi_tmp + 6];
    for (rpy_hat_tmp = 0; rpy_hat_tmp < 3; rpy_hat_tmp++) {
      rtemp = Rdi_tmp + 3 * rpy_hat_tmp;
      B[rtemp] = ((maxval * static_cast<double>(b_a[3 * rpy_hat_tmp]) +
                   a21 * static_cast<double>(b_a[3 * rpy_hat_tmp + 1])) +
                  d * static_cast<double>(b_a[3 * rpy_hat_tmp + 2])) +
                 Rdi[rtemp];
    }
  }
  r1 = 0;
  r2 = 1;
  r3 = 2;
  maxval = std::abs(B[0]);
  a21 = std::abs(B[1]);
  if (a21 > maxval) {
    maxval = a21;
    r1 = 1;
    r2 = 0;
  }
  if (std::abs(B[2]) > maxval) {
    r1 = 2;
    r2 = 1;
    r3 = 0;
  }
  B[r2] /= B[r1];
  B[r3] /= B[r1];
  B[r2 + 3] -= B[r2] * B[r1 + 3];
  B[r3 + 3] -= B[r3] * B[r1 + 3];
  B[r2 + 6] -= B[r2] * B[r1 + 6];
  B[r3 + 6] -= B[r3] * B[r1 + 6];
  if (std::abs(B[r3 + 3]) > std::abs(B[r2 + 3])) {
    rtemp = r2;
    r2 = r3;
    r3 = rtemp;
  }
  B[r3 + 3] /= B[r2 + 3];
  B[r3 + 6] -= B[r3 + 3] * B[r2 + 6];
  //  卡尔曼增益
  b_roll[0] = roll;
  b_roll[1] = pitch;
  b_roll[2] = yaw;
  maxval = rpy_hat[0];
  a21 = rpy_hat[1];
  d = rpy_hat[2];
  for (rpy_hat_tmp = 0; rpy_hat_tmp < 3; rpy_hat_tmp++) {
    int b_Rdi_tmp;
    rtemp = rpy_hat_tmp + 3 * r1;
    Rdi[rtemp] = Qdi[rpy_hat_tmp] / B[r1];
    Rdi_tmp = rpy_hat_tmp + 3 * r2;
    Rdi[Rdi_tmp] = Qdi[rpy_hat_tmp + 3] - Rdi[rtemp] * B[r1 + 3];
    b_Rdi_tmp = rpy_hat_tmp + 3 * r3;
    Rdi[b_Rdi_tmp] = Qdi[rpy_hat_tmp + 6] - Rdi[rtemp] * B[r1 + 6];
    Rdi[Rdi_tmp] /= B[r2 + 3];
    Rdi[b_Rdi_tmp] -= Rdi[Rdi_tmp] * B[r2 + 6];
    Rdi[b_Rdi_tmp] /= B[r3 + 6];
    Rdi[Rdi_tmp] -= Rdi[b_Rdi_tmp] * B[r3 + 3];
    Rdi[rtemp] -= Rdi[b_Rdi_tmp] * B[r3];
    Rdi[rtemp] -= Rdi[Rdi_tmp] * B[r2];
    b_roll[rpy_hat_tmp] -= (static_cast<double>(b_a[rpy_hat_tmp]) * maxval +
                            static_cast<double>(b_a[rpy_hat_tmp + 3]) * a21) +
                           static_cast<double>(b_a[rpy_hat_tmp + 6]) * d;
  }
  maxval = b_roll[0];
  a21 = b_roll[1];
  d = b_roll[2];
  for (Rdi_tmp = 0; Rdi_tmp < 3; Rdi_tmp++) {
    rpy_hat[Rdi_tmp] +=
        (Rdi[Rdi_tmp] * maxval + Rdi[Rdi_tmp + 3] * a21) + Rdi[Rdi_tmp + 6] * d;
  }
  std::memset(&Qdi[0], 0, 9U * sizeof(double));
  Qdi[0] = 1.0;
  Qdi[4] = 1.0;
  Qdi[8] = 1.0;
  for (Rdi_tmp = 0; Rdi_tmp < 3; Rdi_tmp++) {
    maxval = Rdi[Rdi_tmp];
    a21 = Rdi[Rdi_tmp + 3];
    d = Rdi[Rdi_tmp + 6];
    for (rpy_hat_tmp = 0; rpy_hat_tmp < 3; rpy_hat_tmp++) {
      rtemp = Rdi_tmp + 3 * rpy_hat_tmp;
      a[rtemp] =
          Qdi[rtemp] - ((maxval * static_cast<double>(b_a[3 * rpy_hat_tmp]) +
                         a21 * static_cast<double>(b_a[3 * rpy_hat_tmp + 1])) +
                        d * static_cast<double>(b_a[3 * rpy_hat_tmp + 2]));
    }
    maxval = a[Rdi_tmp];
    a21 = a[Rdi_tmp + 3];
    d = a[Rdi_tmp + 6];
    for (rpy_hat_tmp = 0; rpy_hat_tmp < 3; rpy_hat_tmp++) {
      Qdi[Rdi_tmp + 3 * rpy_hat_tmp] =
          (maxval * P[3 * rpy_hat_tmp] + a21 * P[3 * rpy_hat_tmp + 1]) +
          d * P[3 * rpy_hat_tmp + 2];
    }
  }
  std::copy(&Qdi[0], &Qdi[9], &P[0]);
  //
  *roll_f = rpy_hat[0];
  *pitch_f = rpy_hat[1];
  *yaw_f = rpy_hat[2];
  //
  roll_pre = rpy_hat[0];
  pitch_pre = rpy_hat[1];
  yaw_pre = rpy_hat[2];
}

//
// Arguments    : void
// Return Type  : void
//
void rpy_filter_init()
{
  roll_pre = 0.0;
  pitch_pre = 0.0;
  yaw_pre = -1.5707963267948966;
  //  初始状态协方差
  std::memset(&P[0], 0, 9U * sizeof(double));
  P[0] = 1.0;
  P[4] = 1.0;
  P[8] = 1.0;
}

//
// File trailer for rpy_filter.cpp
//
// [EOF]
//
